#!/usr/bin/python3
# -*- coding: utf-8 -*-
# @Time    : 2023/7/13 上午11:07
# @Author  : xiaxiuli
# @FileName: iou.py
import logging

# 修改第207行
from torch import optim, nn

# from yolov7.utils.self_atten import selfAttention

LOGGING_NAME = 'ultralytics'
LOGGER = logging.getLogger(LOGGING_NAME)
import torch, math


class WIoU_Scale:
    """
    monotonous: {
            None: origin v1
            True: monotonic FM v2
            False: non-monotonic FM v3
        }
    momentum: The momentum of running mean
    """

    iou_mean = 1.
    monotonous = False
    _momentum = 1 - 0.5 ** (1 / 7000)
    _is_train = True

    def __init__(self, iou):
        self.iou = iou
        self._update(self)

    @classmethod
    def _update(cls, self):
        if cls._is_train: cls.iou_mean = (1 - cls._momentum) * cls.iou_mean + \
                                         cls._momentum * self.iou.detach().mean().item()

    @classmethod
    def _scaled_loss(cls, self, gamma=1.9, delta=3):
        if isinstance(self.monotonous, bool):
            if self.monotonous:
                return (self.iou.detach() / self.iou_mean).sqrt()
            else:
                beta = self.iou.detach() / self.iou_mean
                alpha = delta * torch.pow(gamma, beta - delta)
                return beta / alpha
        return 1


def gradient_iou(b1_x1, b1_y1, b1_x2, b1_y2, b2_x1, b2_y1, b2_x2, b2_y2, alpha=1, omega=1):
    # 宽和高
    w1, h1 = b1_x2 - b1_x1, (b1_y2 - b1_y1).clamp(omega)
    w2, h2 = b2_x2 - b2_x1, (b2_y2 - b2_y1).clamp(omega)

    # 交集
    inter = (b1_x2.minimum(b2_x2) - b1_x1.maximum(b2_x1)).clamp(0) * \
            (b1_y2.minimum(b2_y2) - b1_y1.maximum(b2_y1)).clamp(0)
    # 并集
    union = w1 * h1 + w2 * h2 - inter + omega

    # 交并比
    grad_iou = torch.pow(inter / (union + omega), alpha)
    return grad_iou


# 动态聚焦系数
def dynamic_focal(x, w=1, b=0, eps=1e-7):
    # x_normed = (x - torch.mean(x, dim=0, keepdim=True)) / torch.std(x, dim=0, keepdim=True)
    # print("--- \n {}".format((torch.var(x, dim=0) + eps) ** 0.5))
    x_normed = w * (x - torch.mean(x, dim=0)) / ((torch.var(x, dim=0) + eps) ** 0.5) + b
    print(x_normed)
    return 1 / (1 + torch.exp(-x_normed))


# 添加可学习参数
# kappa_lr = nn.Parameter(torch.Tensor([1]))
# psi_lr = nn.Parameter(torch.Tensor([0]))


def bbox_iou(box1, box2, x1y1x2y2=True, GIoU=False, DIoU=False, CIoU=False, SIoU=False, EIoU=False, WIoU=False,
             Dynamic=False, LIoU=False, version="1.5.5", Focal=False, alpha=1, gamma=0.5, scale=False, eps=1e-7, ):
    """
    # Returns the IoU of box1 to box2. box1 is 4, box2 is nx4
    """
    box2 = box2.T

    # Get the coordinates of bounding boxes
    if x1y1x2y2:  # x1, y1, x2, y2 = box1
        b1_x1, b1_y1, b1_x2, b1_y2 = box1[0], box1[1], box1[2], box1[3]
        b2_x1, b2_y1, b2_x2, b2_y2 = box2[0], box2[1], box2[2], box2[3]
    else:  # transform from xywh to xyxy
        b1_x1, b1_x2 = box1[0] - box1[2] / 2, box1[0] + box1[2] / 2
        b1_y1, b1_y2 = box1[1] - box1[3] / 2, box1[1] + box1[3] / 2
        b2_x1, b2_x2 = box2[0] - box2[2] / 2, box2[0] + box2[2] / 2
        b2_y1, b2_y2 = box2[1] - box2[3] / 2, box2[1] + box2[3] / 2

    # 交集
    inter = (torch.min(b1_x2, b2_x2) - torch.max(b1_x1, b2_x1)).clamp(0) * \
            (torch.min(b1_y2, b2_y2) - torch.max(b1_y1, b2_y1)).clamp(0)

    # 并集
    w1, h1 = b1_x2 - b1_x1, b1_y2 - b1_y1 + eps
    w2, h2 = b2_x2 - b2_x1, b2_y2 - b2_y1 + eps
    union = w1 * h1 + w2 * h2 - inter + eps

    if scale:
        self = WIoU_Scale(1 - (inter / union))

    # IoU
    # iou = inter / union # ori iou
    iou = torch.pow(inter / (union + eps), alpha)  # alpha iou

    if CIoU or DIoU or GIoU or EIoU or SIoU or WIoU:
        cw = b1_x2.maximum(b2_x2) - b1_x1.minimum(b2_x1)  # convex (smallest enclosing box) width
        ch = b1_y2.maximum(b2_y2) - b1_y1.minimum(b2_y1)  # convex height
        if CIoU or DIoU or EIoU or SIoU or WIoU:  # Distance or Complete IoU https://arxiv.org/abs/1911.08287v1
            c2 = (cw ** 2 + ch ** 2) ** alpha + eps  # convex diagonal squared
            rho2 = (((b2_x1 + b2_x2 - b1_x1 - b1_x2) ** 2 + (
                    b2_y1 + b2_y2 - b1_y1 - b1_y2) ** 2) / 4) ** alpha  # center dist ** 2
            if CIoU:  # https://kgithub.com/Zzh-tju/DIoU-SSD-pytorch/blob/master/utils/box/box_utils.py#L47
                v = (4 / math.pi ** 2) * (torch.atan(w2 / h2) - torch.atan(w1 / h1)).pow(2)
                with torch.no_grad():
                    alpha_ciou = v / (v - iou + (1 + eps))
                if Focal:
                    LOGGER.info(" Focal_CIoU")
                    return iou - (rho2 / c2 + torch.pow(v * alpha_ciou + eps, alpha)), torch.pow(inter / (union + eps),
                                                                                                 gamma)  # Focal_CIoU
                else:
                    LOGGER.info(" CIoU")
                    return iou - (rho2 / c2 + torch.pow(v * alpha_ciou + eps, alpha))  # CIoU
            elif EIoU:
                rho_w2 = ((b2_x2 - b2_x1) - (b1_x2 - b1_x1)) ** 2
                rho_h2 = ((b2_y2 - b2_y1) - (b1_y2 - b1_y1)) ** 2
                cw2 = torch.pow(cw ** 2 + eps, alpha)
                ch2 = torch.pow(ch ** 2 + eps, alpha)
                if Focal:
                    LOGGER.info(" Focal_EIoU")
                    return iou - (rho2 / c2 + rho_w2 / cw2 + rho_h2 / ch2), torch.pow(inter / (union + eps),
                                                                                      gamma)  # Focal_EIou
                else:
                    LOGGER.info(" EIoU")
                    return iou - (rho2 / c2 + rho_w2 / cw2 + rho_h2 / ch2)  # EIou
            elif SIoU:
                # SIoU Loss https://arxiv.org/pdf/2205.12740.pdf
                s_cw = (b2_x1 + b2_x2 - b1_x1 - b1_x2) * 0.5 + eps
                s_ch = (b2_y1 + b2_y2 - b1_y1 - b1_y2) * 0.5 + eps
                sigma = torch.pow(s_cw ** 2 + s_ch ** 2, 0.5)
                sin_alpha_1 = torch.abs(s_cw) / sigma
                sin_alpha_2 = torch.abs(s_ch) / sigma
                threshold = pow(2, 0.5) / 2
                sin_alpha = torch.where(sin_alpha_1 > threshold, sin_alpha_2, sin_alpha_1)
                angle_cost = torch.cos(torch.arcsin(sin_alpha) * 2 - math.pi / 2)
                rho_x = (s_cw / cw) ** 2
                rho_y = (s_ch / ch) ** 2
                gamma = angle_cost - 2
                distance_cost = 2 - torch.exp(gamma * rho_x) - torch.exp(gamma * rho_y)
                omiga_w = torch.abs(w1 - w2) / torch.max(w1, w2)
                omiga_h = torch.abs(h1 - h2) / torch.max(h1, h2)
                shape_cost = torch.pow(1 - torch.exp(-1 * omiga_w), 4) + torch.pow(1 - torch.exp(-1 * omiga_h), 4)
                if Focal:
                    LOGGER.info(" Focal_SIoU")
                    return iou - torch.pow(0.5 * (distance_cost + shape_cost) + eps, alpha), torch.pow(
                        inter / (union + eps), gamma)  # Focal_SIou
                else:
                    LOGGER.info(" SIoU")
                    return iou - torch.pow(0.5 * (distance_cost + shape_cost) + eps, alpha)  # SIou
            elif WIoU:
                if Focal:
                    raise RuntimeError("WIoU do not support Focal.")
                elif scale:
                    LOGGER.info(" WIoU v3")
                    return getattr(WIoU_Scale, '_scaled_loss')(self), (1 - iou) * torch.exp(
                        (rho2 / c2)), iou  # WIoU https://arxiv.org/abs/2301.10051
                else:
                    LOGGER.info(" WIoU v1")
                    return iou, torch.exp((rho2 / c2))  # WIoU v1
            if Focal:
                LOGGER.info(" Focal_DIoU")
                return iou - rho2 / c2, torch.pow(inter / (union + eps), gamma)  # Focal_DIoU
            else:
                LOGGER.info(" DIoU")
                return iou - rho2 / c2  # DIoU rho2 / c2 [3.18176e-03, 3.47125e-03, 4.18841e-05...]
        c_area = cw * ch + eps  # convex area
        if Focal:
            LOGGER.info(" Focal_GIoU")
            return iou - torch.pow((c_area - union) / c_area + eps, alpha), torch.pow(inter / (union + eps),
                                                                                      gamma)  # Focal_GIoU https://arxiv.org/pdf/1902.09630.pdf
        else:
            LOGGER.info(" GIoU")
            return iou - torch.pow((c_area - union) / c_area + eps, alpha)  # GIoU https://arxiv.org/pdf/1902.09630.pdf

    if LIoU:
        # box1为预测框，box2为标记框
        if True:
            # 外接矩形
            cw = b1_x2.maximum(b2_x2) - b1_x1.minimum(b2_x1)  # convex (smallest enclosing box) width
            ch = b1_y2.maximum(b2_y2) - b1_y1.minimum(b2_y1)  # convex height
            cw2 = torch.pow(cw ** 2 + eps, alpha)
            ch2 = torch.pow(ch ** 2 + eps, alpha)
            c2 = (cw ** 2 + ch ** 2) ** alpha + eps  # convex diagonal squared

            # 宽高差、中心距
            rho_w2 = ((b2_x2 - b2_x1) - (b1_x2 - b1_x1)) ** 2
            rho_h2 = ((b2_y2 - b2_y1) - (b1_y2 - b1_y1)) ** 2
            rho2 = (((b2_x1 + b2_x2 - b1_x1 - b1_x2) ** 2 + (
                    b2_y1 + b2_y2 - b1_y1 - b1_y2) ** 2) / 4) ** alpha  # center dist ** 2

        # LIoU版本迭代
        if len(version) != 0:  # Aug_EIoU
            version = "1.5.5"  # 版本号
            kappa = 1  # 超参数
            mu = 1
            psi = 0
            branch = "b2"  # scale
            delta = 3
            zeta = 1.9
            sigma = 1  # 预留

            # 角度
            theta_box1 = torch.atan(h1 / w1)
            theta_box2 = torch.atan(h2 / w2)
            theta_convex = torch.atan(ch / cw)

            # 半径
            radius_box1 = torch.sqrt(h1 ** 2 + w1 ** 2)
            radius_box2 = torch.sqrt(h2 ** 2 + w2 ** 2)
            radius_convex = c2 ** 0.5
            arc_box = ((radius_box2 - radius_box1) * (theta_box2 - theta_box1)) ** 2
            arc_convex = (torch.max((math.pi * 0.5 - theta_convex), theta_convex) * radius_convex) ** 2

            # 损失
            leibniz_iou = iou - rho2 / c2 - arc_box / arc_convex

            # IoU焦点
            if Focal:
                # LOGGER.info(" Focal_LIoU_v%s k:%s m:%s p:%s s:%s g:%s " % (version, kappa, mu, psi, sigma, gamma))
                return leibniz_iou, torch.pow(inter / (union + eps), gamma)
            elif len(branch) != 0:  # 类似Focal-EIoU
                # LIoU
                c_area = cw * ch + eps
                giou = iou - torch.pow((c_area - union) / c_area + eps, 1)
                coef = dynamic_focal(giou, kappa, psi)
                LOGGER.info("[%s] LIoU_v%s k:%s m:%s p:%s d:%s s:%s" % (branch, version, kappa, mu, psi, delta, sigma))

                if branch == "b2":
                    LOGGER.debug("---\ncoef:%s, \ngiou:%s, \nleibniz:%s " % (coef, giou, coef * giou))
                    PM2 = rho2 / c2 + rho_h2 / (4 * ch2) + rho_w2 / (4 * cw2)
                    return (iou - PM2) * coef  # 0923

    if Focal:
        LOGGER.info(" Focal_IoU")
        return iou, torch.pow(inter / (union + eps), gamma)  # Focal_IoU
    else:
        return iou  # IoU
